/*
 * Controller.cpp
 *
 *      Author: Ben, Philippe, Simon en Tom
 */


#include "Robot.h"

int main()
{
   	thread t (server);
   	
	Robot r;
	//init : zie later
	int arc;
	int dis;
	bool distance_ok = false;
	bool arc_ok=false;
	//p = getPacket()
	//r.Robot::UpdateRobot(p);
	//r.Robot::UpdateTarget(p);


	/*TEST CASE*/
	/*Info i;
	i.robx = 600;
	i.roby = 200;
	i.robhoek = 40;
	i.doelx = 100;
	i.doely = 400;
	i.garx = 0;
	i.gary = 0;

	Udp_header h;
	Udp_pakket d;
	d.header = h;
	d.groep_a = 0;
	d.info_a = i;
	d.groep_b = 1;
	d.info_b = i;
	Pakket p;
	p.nr = 0;
	p.d = d;

	r.UpdateRobot(p);
	r.UpdateTarget(p);

	arc = r.CalcTurn();
	cout << arc << endl;
	dis = r.CalcDistance();
	cout << dis << endl;*/


	Package a,b,p;

	b=Buffer::get_package();
	a=b;
	while(!distance_ok)
	{
		printf("begin\n");
	
		while(!arc_ok)
		{
			b=Buffer::get_package();
			//Enkel herbereken indien nieuw pakket aankomt
			if(b.nr<=a.nr) {
				continue;
			}
			a=b;
			
			r.UpdateRobot(b);

			arc = r.CalcTurn();
			cout << arc << endl;
			if(arc==0)
			{
				arc_ok = true;
				break;
			}
			r.Turn(arc);
		}

		dis = r.CalcDistance();
		if(dis == 0)
		{
			distance_ok = true;
			break;
		}
		r.Drive(dis);
		arc_ok = false;
		
	}


	/*in geval van midlevel functies*/
	/*	
	//tweede instantie
	int turnStart();
	int turnStop();

	int driveStart();
	int driveStop();

	bool driving = false;
	bool turning = false;

	//while lus om naar 1 target te rijden
	while(1)
	{
		while(1)
		{
			b=Buffer::get_package();
			if(b.nr<=a.nr) {
				continue;
			}
			a=b;
			r.UpdateRobot(b);
			arc = r.CalcTurn();
			
			if(arc==0)
			{
				turnStop();
				turning = false;				
				break;
			}
			if(driving)
			{
				driving = false;
				driveStop();
			}

			if(!turning)
			{
				turning = true;	
				turnStart();
			}			
		}
		
		dis = r.CalcDistance();
		if(dis == 0)
		{
			driving = false;
			driveStop();
			break;
		}
		
		if(!driving)
		{
			driving = true;
			driveStart();
		}
	}
	*/
	
	//Car in position

	//grijpen
t.join();
}
